#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Bool.h>
#include <localization_monitor/LocalizationStatus.h>
#include <tf2/utils.h>

class OdomConsistencyMonitor {
public:
    OdomConsistencyMonitor() : tf_listener_(tf_buffer_) {
        // 参数初始化
        ros::NodeHandle private_nh("~");
        private_nh.param("max_linear_velocity", max_linear_velocity_, 1.3);
        private_nh.param("max_angular_velocity", max_angular_velocity_, 1.3);
        private_nh.param("position_tolerance", position_tolerance_, 0.1);//位置差
        private_nh.param("angle_tolerance", angle_tolerance_, 1.0);//角度差
        private_nh.param("warning_threshold", warning_threshold_, 1);//警告
        private_nh.param("loss_threshold", loss_threshold_, 2);//连续丢失次数
        private_nh.param("update_interval", update_interval_, 0.05);
        private_nh.param("recovery_timeout", recovery_timeout_, 1.5);//丢失时间
        private_nh.param("odom_stagnant_threshold", odom_stagnant_threshold_, 0.0);// 当里程计位置和角度变化小于此值时，认为里程计停滞
        private_nh.param("carto_stagnant_threshold", carto_stagnant_threshold_, 0.2); // 当Cartographer位置和角度变化小于此值时，认为Cartographer可能丢失
        private_nh.param("direct_position_threshold", direct_position_threshold_, 1.0);
        private_nh.param("direct_angle_threshold", direct_angle_threshold_, 2.0);//绝对差值
        private_nh.param("min_moving_velocity", min_moving_velocity_, 0.1); // 当机器人速度小于此值时，认为机器人处于静止状态
        private_nh.param("stagnant_check_velocity_threshold", stagnant_check_velocity_threshold_, 0.2); // 新增：停滞检查速度阈值
        
        // 坐标系参数
        private_nh.param<std::string>("map_frame", map_frame_, "map");
        private_nh.param<std::string>("odom_frame", odom_frame_, "odom");
        private_nh.param<std::string>("base_frame", base_frame_, "base_footprint");
        
        // 发布者和订阅者
        status_pub_ = nh_.advertise<localization_monitor::LocalizationStatus>("/loc_status", 10);
        warning_pub_ = nh_.advertise<std_msgs::Bool>("/loc_warning", 10);
        odom_sub_ = nh_.subscribe<nav_msgs::Odometry>("/odom", 10, 
                     &OdomConsistencyMonitor::odomCallback, this);
        
        // 初始化状态
        consecutive_errors_ = 0;
        last_valid_pose_.header.frame_id = map_frame_;
        last_valid_pose_.pose.orientation.w = 1.0;
        loss_start_time_ = ros::Time(0);
        confirmed_loss_ = false;
        odom_stagnant_ = false;
        carto_stagnant_ = false;
        is_robot_moving_ = false;
        current_linear_velocity_ = 0.0; // 新增：当前线速度
        
        // 初始化历史数据标志
        has_last_odom_pose_ = false;
        has_last_carto_pose_ = false;
        
        ROS_INFO("Odom Consistency Monitor initialized");
        
        timer_ = nh_.createTimer(ros::Duration(update_interval_), 
                                &OdomConsistencyMonitor::checkConsistency, this);
    }

    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
        boost::mutex::scoped_lock lock(odom_mutex_);
        latest_odom_ = *msg;
        
        // 检查机器人是否在运动
        double linear_velocity = std::sqrt(
            msg->twist.twist.linear.x * msg->twist.twist.linear.x +
            msg->twist.twist.linear.y * msg->twist.twist.linear.y);
        double angular_velocity = std::abs(msg->twist.twist.angular.z);
        
        // 保存当前线速度
        current_linear_velocity_ = linear_velocity;
        
        // 判断是否达到运动阈值
        is_robot_moving_ = (linear_velocity > min_moving_velocity_ || 
                           angular_velocity > min_moving_velocity_);
        
        // 检查里程计是否停滞
        if (has_last_odom_pose_) {
            double dx = msg->pose.pose.position.x - last_odom_pose_.position.x;
            double dy = msg->pose.pose.position.y - last_odom_pose_.position.y;
            double position_diff = std::sqrt(dx*dx + dy*dy);
            
            double last_yaw = tf2::getYaw(last_odom_pose_.orientation);
            double current_yaw = tf2::getYaw(msg->pose.pose.orientation);
            double angle_diff = std::abs(current_yaw - last_yaw);
            angle_diff = (angle_diff > M_PI) ? (2*M_PI - angle_diff) : angle_diff;
            
            // 判断是否停滞（位置和角度变化均小于阈值）
            odom_stagnant_ = (position_diff < odom_stagnant_threshold_);
        }
        last_odom_pose_ = msg->pose.pose;
        has_last_odom_pose_ = true;
    }
    
    // 核心检查函数（定时触发）
    void checkConsistency(const ros::TimerEvent& event) {
        if (!tf_buffer_.canTransform(map_frame_, base_frame_, ros::Time(0), ros::Duration(3)) ||
            !tf_buffer_.canTransform(map_frame_, odom_frame_, ros::Time(0), ros::Duration(3))) {
            ROS_WARN_THROTTLE(5.0, "Waiting for necessary transforms to become available...");
            return;
        }
        
        // 获取最新里程计数据和当前线速度（线程安全）
        nav_msgs::Odometry current_odom;
        double current_linear_velocity;
        {
            boost::mutex::scoped_lock lock(odom_mutex_);
            current_odom = latest_odom_;
            current_linear_velocity = current_linear_velocity_;
        }
        
        ros::Time now = ros::Time::now();
        
        // 1. 获取Cartographer的map->base_footprint位姿
        geometry_msgs::PoseStamped carto_pose;
        try {
            geometry_msgs::TransformStamped tf_map_to_base = tf_buffer_.lookupTransform(
                map_frame_, base_frame_, ros::Time(0), ros::Duration(5.0));
            
            carto_pose.header = tf_map_to_base.header;
            carto_pose.pose.position.x = tf_map_to_base.transform.translation.x;
            carto_pose.pose.position.y = tf_map_to_base.transform.translation.y;
            carto_pose.pose.position.z = tf_map_to_base.transform.translation.z;
            carto_pose.pose.orientation = tf_map_to_base.transform.rotation;
        } catch (tf2::TransformException &ex) {
            ROS_WARN_THROTTLE(1.0, "TF lookup (map->%s) failed: %s", base_frame_.c_str(), ex.what());
            return;
        }
        
        // 只有当线速度大于阈值时才检查Cartographer是否停滞
        if (current_linear_velocity > stagnant_check_velocity_threshold_) {
            if (has_last_carto_pose_) {
                double dx = carto_pose.pose.position.x - last_carto_pose_.position.x;
                double dy = carto_pose.pose.position.y - last_carto_pose_.position.y;
                double position_diff = std::sqrt(dx*dx + dy*dy);
                
                double last_yaw = tf2::getYaw(last_carto_pose_.orientation);
                double current_yaw = tf2::getYaw(carto_pose.pose.orientation);
                double angle_diff = std::abs(current_yaw - last_yaw);
                angle_diff = (angle_diff > M_PI) ? (2*M_PI - angle_diff) : angle_diff;
                
                carto_stagnant_ = (position_diff < carto_stagnant_threshold_);
            }
        } else {
            // 线速度低于阈值，不进行Cartographer停滞检测
            carto_stagnant_ = false;
        }
        
        last_carto_pose_ = carto_pose.pose;
        has_last_carto_pose_ = true;
        
        // 2. 获取里程计的map->base_footprint位姿 (组合map->odom + odom->base)
        geometry_msgs::PoseStamped odom_pose_map_frame;
        try {
            // 获取map->odom变换 (Cartographer输出的修正)
            geometry_msgs::TransformStamped tf_map_to_odom = tf_buffer_.lookupTransform(
                map_frame_, odom_frame_, ros::Time(0), ros::Duration(5.0));
            
            // 创建odom坐标系下的位姿
            geometry_msgs::PoseStamped odom_pose_odom_frame;
            odom_pose_odom_frame.header.frame_id = odom_frame_;
            odom_pose_odom_frame.header.stamp = now;
            odom_pose_odom_frame.pose = current_odom.pose.pose;
            
            // 转换到map坐标系
            tf2::doTransform(odom_pose_odom_frame, odom_pose_map_frame, tf_map_to_odom);
        } catch (tf2::TransformException &ex) {
            ROS_WARN_THROTTLE(1.0, "TF lookup (map->odom) failed: %s", ex.what());
            return;
        }
        
        // 3. 在map坐标系下比较两个位姿
        double dx = carto_pose.pose.position.x - odom_pose_map_frame.pose.position.x;
        double dy = carto_pose.pose.position.y - odom_pose_map_frame.pose.position.y;
        double position_diff = std::sqrt(dx*dx + dy*dy);
        
        double carto_yaw = tf2::getYaw(carto_pose.pose.orientation);
        double odom_yaw = tf2::getYaw(odom_pose_map_frame.pose.orientation);
        double angle_diff = std::abs(carto_yaw - odom_yaw);
        angle_diff = (angle_diff > M_PI) ? (2*M_PI - angle_diff) : angle_diff;
        
        // 计算最大允许差异
        double max_position_diff = max_linear_velocity_ * update_interval_ + position_tolerance_;
        double max_angle_diff = max_angular_velocity_ * update_interval_ + angle_tolerance_;
        
        // 创建状态消息
        std_msgs::Bool warn_msg;
        localization_monitor::LocalizationStatus status_msg;
        status_msg.header.stamp = now;
        status_msg.position_diff = position_diff;
        status_msg.angle_diff = angle_diff;
        status_msg.max_position_diff = max_position_diff;
        status_msg.max_angle_diff = max_angle_diff;
        
        // 检查一致性 - 添加多种检测条件
        bool consistency_error = (position_diff > max_position_diff || angle_diff > max_angle_diff);
        bool direct_error = (position_diff > direct_position_threshold_ || angle_diff > direct_angle_threshold_);
        bool odom_issue = odom_stagnant_;
        bool carto_issue = carto_stagnant_; // 使用更有意义的变量名
        
        // 如果直接误差很大，立即触发错误
        if (direct_error) {
            consistency_error = true;
        }
        
        // 只有当机器人在运动时才检测定位丢失
        if (is_robot_moving_ && (consistency_error || odom_issue || carto_issue)) {
            consecutive_errors_++;
            
            // 如果是首次达到丢失阈值，记录开始时间
            if (consecutive_errors_ >= loss_threshold_ && loss_start_time_.isZero()) {
                loss_start_time_ = ros::Time::now();
                ROS_WARN("first loss");
            }
            
            // 检查是否已经超过恢复超时时间
            if (!loss_start_time_.isZero() && 
                (now - loss_start_time_).toSec() >= recovery_timeout_) {
                // 超时未恢复则确认丢失
                if (!confirmed_loss_) {
                    confirmed_loss_ = true;
                    
                    if (direct_error) {
                        ROS_ERROR("Localization LOST (direct large error)! "
                                  "Pos diff: %.3fm > %.3fm, Ang diff: %.3frad > %.3frad", 
                                  position_diff, direct_position_threshold_, angle_diff, direct_angle_threshold_);
                    } else if (consistency_error) {
                        ROS_ERROR("Localization LOST (odometry consistency)! "
                                  "Pos diff: %.3fm > %.3fm, Ang diff: %.3frad > %.3frad", 
                                  position_diff, max_position_diff, angle_diff, max_angle_diff);
                    } else if (odom_issue) {
                        ROS_ERROR("Localization LOST (odometry stagnant)! "
                                 "Odom is not updating properly");
                    } else if (carto_issue) {
                        ROS_ERROR("Localization LOST (cartographer stagnant)! "
                                 "Cartographer is not updating properly");
                    }
                    
                    status_msg.is_lost = true;
                    warn_msg.data = true;
                    status_msg.status_level = localization_monitor::LocalizationStatus::STATUS_LOST;
                }
                
                if (direct_error) {
                    status_msg.loss_reason = "direct_large_error";
                } else if (consistency_error) {
                    status_msg.loss_reason = "odometry_consistency_failed";
                } else if (odom_issue) {
                    status_msg.loss_reason = "odometry_stagnant";
                } else if (carto_issue) {
                    status_msg.loss_reason = "cartographer_stagnant";
                }
                
                warn_msg.data = true;
                status_msg.is_lost = true;
                status_pub_.publish(status_msg);
                warning_pub_.publish(warn_msg);
            } 
            // 达到警告阈值但未超时
            else if (consecutive_errors_ >= warning_threshold_) {
                warn_msg.data = false;
                status_msg.is_lost = false;
                status_msg.status_level = localization_monitor::LocalizationStatus::STATUS_WARNING;
                
                if (direct_error) {
                    status_msg.loss_reason = "direct_large_error_warning";
                    ROS_WARN("Localization WARNING! Direct large error: Pos diff: %.3fm > %.3fm, Ang diff: %.3frad > %.3frad", 
                             position_diff, direct_position_threshold_, angle_diff, direct_angle_threshold_);
                } else if (consistency_error) {
                    status_msg.loss_reason = "odometry_consistency_warning";
                    ROS_WARN("Localization WARNING! Pos diff: %.3fm > %.3fm, Ang diff: %.3frad > %.3frad", 
                             position_diff, max_position_diff, angle_diff, max_angle_diff);
                } else if (odom_issue) {
                    status_msg.loss_reason = "odometry_stagnant_warning";
                    ROS_WARN("Localization WARNING! Odometry stagnant detected");
                } else if (carto_issue) {
                    status_msg.loss_reason = "cartographer_stagnant_warning";
                    ROS_WARN("Localization WARNING! Cartographer stagnant detected");
                }
            }
        } 
        else {
            // 如果机器人没有运动，重置错误计数
            if (consecutive_errors_ > 0) {
                ROS_DEBUG("Robot is not moving, resetting error count");
                consecutive_errors_ = 0;
            }
            
            // 如果之前处于丢失状态但已恢复
            if (!loss_start_time_.isZero() || confirmed_loss_) {
                ROS_INFO("Localization RECOVERED after %.1f seconds.", 
                         (now - loss_start_time_).toSec());
            }
            
            // 重置所有丢失相关状态
            consecutive_errors_ = 0;
            loss_start_time_ = ros::Time(0);
            confirmed_loss_ = false;
            last_valid_pose_ = carto_pose;
            
            status_msg.is_lost = false;
            warn_msg.data = false;
            status_msg.status_level = localization_monitor::LocalizationStatus::STATUS_NORMAL;
            status_msg.loss_reason = "";
        }
        
        status_msg.last_valid_pose = last_valid_pose_;
        status_pub_.publish(status_msg);
        warning_pub_.publish(warn_msg);
    }

private:
    ros::NodeHandle nh_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    
    ros::Publisher status_pub_;
    ros::Publisher warning_pub_;
    ros::Subscriber odom_sub_;
    ros::Timer timer_;
    
    nav_msgs::Odometry latest_odom_;
    boost::mutex odom_mutex_;
    
    geometry_msgs::PoseStamped last_valid_pose_;
    geometry_msgs::Pose last_carto_pose_; // 上一次Cartographer位姿
    geometry_msgs::Pose last_odom_pose_;  // 上一次里程计位姿
    
    // 标志位，指示是否有上一次的位姿数据
    bool has_last_carto_pose_;
    bool has_last_odom_pose_;
    
    // 参数
    double max_linear_velocity_;
    double max_angular_velocity_;
    double position_tolerance_;
    double angle_tolerance_;
    int warning_threshold_;
    int loss_threshold_;
    double update_interval_;
    double recovery_timeout_;
    double odom_stagnant_threshold_;
    double carto_stagnant_threshold_;
    double direct_position_threshold_;
    double direct_angle_threshold_;
    double min_moving_velocity_;
    double stagnant_check_velocity_threshold_; // 新增：停滞检查速度阈值
    
    std::string map_frame_;
    std::string odom_frame_;
    std::string base_frame_;
    
    int consecutive_errors_;
    ros::Time loss_start_time_;
    bool confirmed_loss_;
    bool odom_stagnant_;
    bool carto_stagnant_;
    bool is_robot_moving_;
    double current_linear_velocity_; // 新增：当前线速度
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "odom_consistency_monitor");
    OdomConsistencyMonitor monitor;
    ros::spin();
    return 0;
}